{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

  {# Handwritten methods for Pose3 #}
  template <typename Derived>
  Pose3(const Rot3<Scalar>& rotation, const Eigen::MatrixBase<Derived>& position) {
    static_assert(Derived::RowsAtCompileTime == 3, "Position must be a 3x1 vector");
    static_assert(Derived::ColsAtCompileTime == 1, "Position must be a 3x1 vector");
    data_.template head<4>() = rotation.Data();
    data_.template tail<3>() = position;
  }

  // Generate a random element, with normally distributed position
  template <typename Generator>
  static Pose3 Random(Generator& gen) {
    // This cannot be combined into Pose3(Rot3::Random(gen), sym::StorageOps::Random(gen)),
    // because the standard does not guarantee evaluation order of arguments,
    // meaning that we would get different results on different compilers.
    const auto rot3 = Rot3<Scalar>::Random(gen);
    const auto vec3 = sym::StorageOps<Vector3>::Random(gen);
    return Pose3(rot3, vec3);
  }

  Eigen::Transform<Scalar, 3, Eigen::TransformTraits::Isometry> ToTransform() const {
    return Eigen::Transform<Scalar, 3, Eigen::TransformTraits::Isometry>{ToHomogenousMatrix()};
  }

  sym::Rot3<Scalar> Rotation() const {
    return sym::Rot3<Scalar>(RotationStorage(), /* normalize */ false);
  }
